import sensor, image, time, pyb, math
from pyb import UART

def main():
    try:
        # 初始化串口通信（UART3，波特率115200）
        uart = UART(3, 115200)
        uart.init(115200, bits=8, parity=None, stop=1)

        # 初始化摄像头（视角调整为正向）
        sensor.reset()
        sensor.set_pixformat(sensor.RGB565)  # 使用RGB565格式
        sensor.set_framesize(sensor.QVGA)    # 分辨率320x240
        sensor.skip_frames(time=1000)        # 等待摄像头稳定
        sensor.set_vflip(False)              # 关闭垂直翻转（正向视角）
        sensor.set_hmirror(False)            # 关闭水平镜像（正向视角）
        sensor.set_auto_gain(True)           # 自动增益
        sensor.set_auto_whitebal(True)       # 自动白平衡

        # 将坐标（0，0）的位置调整到中心点
        IMAGE_CENTER_X = 160
        IMAGE_CENTER_Y = 100


        # 初始化LED指示灯（全部开启）
        leds = [pyb.LED(i) for i in range(1, 4)]
        for led in leds:
            led.on()

        # 色块检测参数配置
        threshold_blackblob = [(0, 32, -38, 2, -65, 37)]  # 黑色色块阈值
        min_area = 80                                 # 最小面积
        max_area = 20000                              # 最大面积
        max_density = 0.35                            # 最大密度
        max_solidity = 0.55                           # 最大坚实度
        max_convexity = 0.65                          # 最大凸度

        # 计时对象（用于计算帧率）
        clock = time.clock()

        # 上次发送的坐标值，用于检测变化
        last_sent_dx = None
        last_sent_dy = None

        # 抖动抑制参数
        history_size = 5                  # 历史数据长度
        dx_history = [0] * history_size   # dx历史数据
        dy_history = [0] * history_size   # dy历史数据
        history_index = 0                 # 历史数据索引
        smooth_factor = 0.2               # 平滑因子 (0-1)
        last_smooth_dx = 0                # 上次平滑后的dx
        last_smooth_dy = 0                # 上次平滑后的dy
        stability_threshold = 2           # 稳定性阈值

        # 主循环
        while True:
            recognized = False  # 标记是否识别到矩形
            dx, dy = 0, 0       # 初始化坐标值
            clock.tick()
            valid_blobs = []

            # 获取图像并检测色块
            img = sensor.snapshot()
            blobs = img.find_blobs(
                threshold_blackblob,
                pixels_threshold=1,
                area_threshold=1,
                x_stride=2,
                y_stride=1,
                margin=16
            )

            # 筛选有效色块
            if blobs:
                for blob in blobs:
                    if (min_area < blob.area() < max_area and
                        is_valid_shape(blob, max_density, max_solidity, max_convexity)):
                        valid_blobs.append(blob)
                        # 调试信息可以注释掉以提高性能
                        # print(f"面积: {blob.area()}, 长宽比: {blob.w()/blob.h():.2f}")
                        # print(f"密度: {blob.density():.3f}, 实心度: {blob.solidity():.3f}")

            # 处理有效色块
            if valid_blobs:
                # 选择面积最大且密度最小的稳定色块
                # 首先按面积排序，选择面积较大的色块
                large_blobs = [blob for blob in valid_blobs if blob.area() > min_area * 2]
                if large_blobs:
                    target_blob = min(large_blobs, key=lambda b: b.density())
                else:
                    target_blob = min(valid_blobs, key=lambda b: b.density())

                try:
                    # 计算靶心和轴比例
                    target_center, axis_ratio = process_blob(target_blob)

                    # 绘制主轴和副轴
                    img.draw_line(target_blob.major_axis_line())
                    img.draw_line(target_blob.minor_axis_line())

                    # 绘制色块信息
                    draw_blob_info(img, target_blob, target_center)

                    # 符合矩形特征时更新坐标
                    if 0.65 < axis_ratio < 0.85:  # 更严格的轴比例要求
                        recognized = True
                        # 计算矩形中心相对于屏幕中心的差值
                        raw_dx = int(target_center[0]) - IMAGE_CENTER_X
                        raw_dy = int(target_center[1]) - IMAGE_CENTER_Y

                        # 添加到历史数据
                        dx_history[history_index] = raw_dx
                        dy_history[history_index] = raw_dy
                        history_index = (history_index + 1) % history_size

                        # 计算移动平均
                        avg_dx = sum(dx_history) // history_size
                        avg_dy = sum(dy_history) // history_size

                        # 应用低通滤波（指数平滑）
                        smooth_dx = int(smooth_factor * last_smooth_dx + (1 - smooth_factor) * avg_dx)
                        smooth_dy = int(smooth_factor * last_smooth_dy + (1 - smooth_factor) * avg_dy)

                        # 稳定性检查：如果变化太小，保持上次值
                        if abs(smooth_dx - last_smooth_dx) < stability_threshold:
                            smooth_dx = last_smooth_dx
                        if abs(smooth_dy - last_smooth_dy) < stability_threshold:
                            smooth_dy = last_smooth_dy

                        # 更新最终坐标
                        dx = smooth_dx
                        dy = smooth_dy

                        # 如果dx或dy接近于2，则将其设置为0
                        if abs(dx) <= 2:
                            dx = 0
                        if abs(dy) <= 2:
                            dy = 0
                        last_smooth_dx = smooth_dx
                        last_smooth_dy = smooth_dy

                except Exception as e:
                    recognized = False
                    dx, dy = 0, 0
            else:
                # 未识别到色块时，逐渐衰减历史数据
                for i in range(history_size):
                    dx_history[i] = int(dx_history[i] * 0.9)
                    dy_history[i] = int(dy_history[i] * 0.9)

            # 检查dx或dy是否有变化，有变化则发送
            has_change = (last_sent_dx is None or last_sent_dy is None or
                         dx != last_sent_dx or dy != last_sent_dy)

            if has_change:
                # 构建并发送数据包
                try:
                    header = 0xFF                                    # 包头
                    flag = 0xFF if recognized else 0x00            # 识别标志
                    footer = 0xFA                                   # 包尾

                    # 将dx和dy限制在-128到127范围内（有符号字节）
                    dx_byte = max(-128, min(127, dx)) & 0xFF
                    dy_byte = max(-128, min(127, dy)) & 0xFF

                    # 构建5字节数据包：包头 + 识别标志 + dx + dy + 包尾
                    data_packet = bytearray([header, flag, dx_byte, dy_byte, footer])
                    uart.write(data_packet)

                    # 打印发送的数据包内容
                    hex_packet = ' '.join([f"0x{x:02X}" for x in data_packet])
                    status = "识别到矩形" if recognized else "未识别到矩形"
                    print(f"发送: {hex_packet} ({status}, dx:{dx}, dy:{dy})")

                    # 清除UART接收缓冲区
                    while uart.any():
                        uart.readchar()

                    # 更新上次发送的坐标值
                    last_sent_dx = dx
                    last_sent_dy = dy
                except Exception as e:
                    pass  # 静默处理发送错误

    except Exception as e:
        # 发生异常时关闭LED
        for led in leds:
            led.off()
    finally:
        # 程序结束时关闭LED
        for led in leds:
            led.off()

def line_intersection(line1, line2):
    """计算两条直线的交点坐标"""
    (x1, y1, x2, y2) = line1
    (x3, y3, x4, y4) = line2

    # 计算分母（判断是否平行）
    den = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
    if den == 0:
        # 平行或重合时返回中线点
        return (abs(x2 - x1) // 2, abs(y2 - y1) // 2)

    # 计算交点参数
    t_num = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)
    t = t_num / den

    # 计算交点坐标
    x = x1 + t * (x2 - x1)
    y = y1 + t * (y2 - y1)

    return (x, y)

def process_blob(blob):
    """处理单个有效色块，返回靶心坐标和轴比例"""
    # 获取主轴和副轴
    major_axis = blob.major_axis_line()
    minor_axis = blob.minor_axis_line()

    # 计算轴交点（靶心）
    intersection = line_intersection(major_axis, minor_axis)

    # 计算轴长比例
    lx1 = abs(major_axis[0] - major_axis[2])
    ly1 = abs(major_axis[1] - major_axis[3])
    l1 = math.sqrt(lx1**2 + ly1**2)

    lx2 = abs(minor_axis[0] - minor_axis[2])
    ly2 = abs(minor_axis[1] - minor_axis[3])
    l2 = math.sqrt(lx2**2 + ly2**2)

    if l1 == 0:  # 防止除以零错误
        return intersection, 0

    return intersection, (l2 / l1)

def draw_blob_info(img, blob, intersection):
    """在图像上绘制色块信息"""
    # 绘制特征角点
    corners = blob.min_corners()
    for (x, y) in corners:
        img.draw_cross(int(x), int(y))

    # 绘制包围圆和靶心
    img.draw_circle(blob.enclosing_circle(), color=(255, 255, 0), thickness=2)
    img.draw_cross(int(intersection[0]), int(intersection[1]), color=(255, 0, 0))

def is_valid_shape(blob, max_density, max_solidity, max_convexity):
    """验证色块形状是否符合要求"""
    # 计算长宽比，避免除以零
    if blob.h() == 0:
        return False
    aspect_ratio = blob.w() / blob.h()

    # 更严格的验证条件，提高稳定性
    return (1.3 < aspect_ratio < 2.2 and           # 更严格的长宽比
            blob.roundness() > 0.25 and            # 提高圆度要求
            blob.density() < max_density and       # 密度要求
            blob.solidity() < max_solidity and     # 实心度要求
            blob.convexity() < max_convexity and   # 凸度要求
            blob.area() > 100)                     # 最小面积要求

# 启动主程序
if __name__ == "__main__":
    main()
